#include <cv_bridge/cv_bridge.h>
#include <glog/logging.h>
#include <image_transport/image_transport.h>
#include <ros/ros.h>
#include <signal.h>

#include <chrono>
#include <opencv2/opencv.hpp>
#include <thread>

#include "commons/rabbit_consts.h"

void MySigintHandler(int sig) {
  ROS_INFO("shutting down!");
  ros::shutdown();
  exit(0);
}

int main(int argc, char** argv) {
  // ros init
  ros::init(argc, argv, "camera_berfore");
  ros::NodeHandle nh;
  image_transport::ImageTransport it(nh);
  signal(SIGINT, MySigintHandler);
  DLOG(INFO) << "Init Ros";
  image_transport::Publisher pub = it.advertise(TOPIC_CAMERA, 1);

  // glog init
  FLAGS_log_dir = BASE_DATA_PATH + "logs/camera_berfore";
  FLAGS_logtostderr = false;
  FLAGS_alsologtostderr = true;
  FLAGS_stderrthreshold = 0;
  google::InitGoogleLogging("camera_berfore");
  DLOG(INFO) << "Debug Mode...";

  while (nh.ok()) {
    cv::VideoCapture cap;
    try {
      cap.open(SMALL_CAR_BEFORE_DEV); 
    //  cap.open(0);
      cap.set(cv::CAP_PROP_FRAME_WIDTH, BIG_VIDEO_HIGHT);
      cap.set(cv::CAP_PROP_FRAME_HEIGHT, BIG_VIDEO_WIDTH);
      cap.set(cv::CAP_PROP_FPS, 30);
     
    } catch (...) {
    }
    DLOG(INFO) << "Create cap";

    if (cap.isOpened()) {
      cv::Mat frame;
      bool flag;
      auto start_time = std::chrono::system_clock::now();
      sensor_msgs::ImagePtr msg;
      ros::Rate loop_rate(50);
      while (nh.ok()) {
        flag = cap.read(frame);
        auto read_time = std::chrono::system_clock::now();
        LOG(INFO) << "read img (" << frame.cols << ", " << frame.rows << ")";
        if (!flag) break;
        cv::resize(frame, frame, cv::Size(BIG_VIDEO_HIGHT, BIG_VIDEO_WIDTH));
        auto resize_time = std::chrono::system_clock::now();
        msg =
            cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
        msg->header.stamp = ros::Time::now();
        pub.publish(msg);
        auto publish_time = std::chrono::system_clock::now();
        ros::spinOnce();
        loop_rate.sleep();
        auto end_time = std::chrono::system_clock::now();

        DLOG(INFO) << "read "
                   << std::chrono::duration_cast<std::chrono::milliseconds>(
                          read_time - start_time)
                          .count()
                   << " ms, resize "
                   << std::chrono::duration_cast<std::chrono::milliseconds>(
                          resize_time - read_time)
                          .count()
                   << " ms, publish "
                   << std::chrono::duration_cast<std::chrono::milliseconds>(
                          publish_time - resize_time)
                          .count()
                   << " ms, sleep "
                   << std::chrono::duration_cast<std::chrono::milliseconds>(
                          end_time - publish_time)
                          .count();
        start_time = end_time;
      }

      DLOG(INFO) << "cap is broken";

      frame.release();
    }
    std::this_thread::sleep_for(std::chrono::seconds(5));
    cap.release();
  }
}
